#include <ros/ros.h>
#include <slam_2d_ros/slam_2d_node.h>
#include <slam_2d_ros/mapping/occupancy_map.h>

class TestOccupancyGrid{
    public:
        TestOccupancyGrid();
        ~TestOccupancyGrid();

        // 雷达回调函数
        void ScanRawCallback(const sensor_msgs::LaserScan::Ptr& msg);

    private:
        ros::NodeHandle nh_;
        ros::Subscriber scanRawSub_;
        slam_2d::Scan2DPtr scanRawPtr_;
};

TestOccupancyGrid::TestOccupancyGrid(){
    scanRawSub_ = nh_.subscribe("/scan_raw", 10, &TestOccupancyGrid::ScanRawCallback, this);
}

TestOccupancyGrid::~TestOccupancyGrid(){

}


// 雷达回调函数
void TestOccupancyGrid::ScanRawCallback(const sensor_msgs::LaserScan::Ptr& msg){
    scanRawPtr_ = msg;

    slam_2d::OccupancyMap occupancyMap_;
    auto curr_frame = boost::make_shared<slam_2d::Frame>(msg);
    occupancyMap_.AddLidarFrame(curr_frame);

    cv::Mat cv_map = occupancyMap_.GetOccupancyGridBlackWhite();
    cv::imshow("occupancy map", cv_map);
    cv::waitKey(10);
}



int main(int argc, char** argv){
    FLAGS_alsologtostderr = true;           // 日志同时输出到stderr
    FLAGS_max_log_size = 1;                 // 单个日志文件大小上限（MB）, 如果设置为0将默认为1
	FLAGS_stop_logging_if_full_disk = true; // 当磁盘写满时，停止输出
    FLAGS_minloglevel = google::WARNING;    // 只记录大于{}等级的日志
    FLAGS_log_dir = "/home/xxty/user_config/logs";
    google::InitGoogleLogging(argv[0]);

    ros::init(argc, argv, "test_occupancy_grid");
    setlocale(LC_ALL,"");   //ROS_INFO 中文显示, 需要添加头文件#include <sstream>
    ROS_INFO("test_occupancy_grid start...");

    TestOccupancyGrid test_occupancy_grid;
    ros::spin();
    google::ShutdownGoogleLogging();
    return 0;
}